% 生成两个圆的参数
radius1 = 1;  % 第一个圆的半径
radius2 = 1;  % 第二个圆的半径

% 第一个圆的中心在坐标轴左边
center1 = [-1, 0];

% 第二个圆的中心在坐标轴右边
center2 = [1, 0];

path_x = [];
path_y = [];
ref_x = [];
ref_y = [];
ref_theta = [];

%theta1 = linspace(0, 2*pi, 100);
%theta2 = linspace(pi, -pi, 100);
%x2 = center2(1) + radius2 * cos(theta2);
%y2 = center2(2) + radius2 * sin(theta2);

%% 第一段半圆
% 生成圆的角度
theta1 = linspace(0, pi, 50);

% 生成圆的坐标
x = center1(1) + radius1 * cos(theta1);
y = center1(2) + radius1 * sin(theta1);

% 中心化和缩放
x_mean = mean(x);
y_mean = mean(y);
x_std = std(x);
y_std = std(y);

x_circle_scaled = (x - x_mean) / x_std;
y_circle_scaled = (y - y_mean) / y_std;

% 合并两个圆，形成8字形轨迹
path_x = [path_x, x];
path_y = [path_y, y];

% 用多项式进行拟合
degree = 19;
poly = polyfit(x_circle_scaled, y_circle_scaled, degree);

% 生成拟合的轨迹
ref_seq = linspace(x_circle_scaled(1), x_circle_scaled(end), 100);
ref_theta_seq = pi+atan2(polyval(polyder(poly), ref_seq),1);% 偏航角
ref_theta_seq = angle_bound(ref_theta_seq);
ref_x = [ref_x, ref_seq * x_std + x_mean]; % 参考轨迹的x坐标
ref_y = [ref_y, polyval(poly, ref_seq) * y_std + y_mean]; % 参考轨迹的y坐标
ref_theta = [ref_theta, ref_theta_seq]; % 参考轨迹的偏航角

%% 第二段半圆
% 生成圆的角度
theta1 = linspace(pi, 2*pi, 50);

% 生成圆的坐标
x = center1(1) + radius1 * cos(theta1);
y = center1(2) + radius1 * sin(theta1);

% 中心化和缩放
x_mean = mean(x);
y_mean = mean(y);
x_std = std(x);
y_std = std(y);

x_circle_scaled = (x - x_mean) / x_std;
y_circle_scaled = (y - y_mean) / y_std;

% 合并两个圆，形成8字形轨迹
path_x = [path_x, x];
path_y = [path_y, y];

% 用多项式进行拟合
degree = 19;
poly = polyfit(x_circle_scaled, y_circle_scaled, degree);

% 生成拟合的轨迹
ref_seq = linspace(x_circle_scaled(1), x_circle_scaled(end), 100);
ref_theta_seq = atan2(polyval(polyder(poly), ref_seq),1);% 偏航角
ref_theta_seq = angle_bound(ref_theta_seq);
ref_x = [ref_x, ref_seq * x_std + x_mean]; % 参考轨迹的x坐标
ref_y = [ref_y, polyval(poly, ref_seq) * y_std + y_mean]; % 参考轨迹的y坐标
ref_theta = [ref_theta, ref_theta_seq]; % 参考轨迹的偏航角

%% 第三段半圆
% 生成圆的角度
theta2 = linspace(pi, 0, 50);

% 生成圆的坐标
x = center2(1) + radius2 * cos(theta2);
y = center2(2) + radius2 * sin(theta2);

% 中心化和缩放
x_mean = mean(x);
y_mean = mean(y);
x_std = std(x);
y_std = std(y);

x_circle_scaled = (x - x_mean) / x_std;
y_circle_scaled = (y - y_mean) / y_std;

% 合并两个圆，形成8字形轨迹
path_x = [path_x, x];
path_y = [path_y, y];

% 用多项式进行拟合
degree = 19;
poly = polyfit(x_circle_scaled, y_circle_scaled, degree);

% 生成拟合的轨迹
ref_seq = linspace(x_circle_scaled(1), x_circle_scaled(end), 100);
ref_theta_seq = atan2(polyval(polyder(poly), ref_seq),1);% 偏航角
ref_theta_seq = angle_bound(ref_theta_seq);
ref_x = [ref_x, ref_seq * x_std + x_mean]; % 参考轨迹的x坐标
ref_y = [ref_y, polyval(poly, ref_seq) * y_std + y_mean]; % 参考轨迹的y坐标
ref_theta = [ref_theta, ref_theta_seq]; % 参考轨迹的偏航角

%% 第四段半圆
% 生成圆的角度
theta2 = linspace(0, -pi, 50);

% 生成圆的坐标
x = center2(1) + radius2 * cos(theta2);
y = center2(2) + radius2 * sin(theta2);

% 中心化和缩放
x_mean = mean(x);
y_mean = mean(y);
x_std = std(x);
y_std = std(y);

x_circle_scaled = (x - x_mean) / x_std;
y_circle_scaled = (y - y_mean) / y_std;

% 合并两个圆，形成8字形轨迹
path_x = [path_x, x];
path_y = [path_y, y];

% 用多项式进行拟合
degree = 19;
poly = polyfit(x_circle_scaled, y_circle_scaled, degree);

% 生成拟合的轨迹
ref_seq = linspace(x_circle_scaled(1), x_circle_scaled(end), 100);
ref_theta_seq = -pi + atan2(polyval(polyder(poly), ref_seq),1);% 偏航角
ref_theta_seq = angle_bound(ref_theta_seq);
ref_x = [ref_x, ref_seq * x_std + x_mean]; % 参考轨迹的x坐标
ref_y = [ref_y, polyval(poly, ref_seq) * y_std + y_mean]; % 参考轨迹的y坐标
ref_theta = [ref_theta, ref_theta_seq]; % 参考轨迹的偏航角

%% 绘制原始轨迹和拟合结果
figure;
plot(path_x, path_y, 'r.', 'LineWidth', 0.8);
hold on;
plot(ref_x, ref_y, 'b.', 'LineWidth', 0.8);
%plot(path_x, fit_y, 'b.', 'LineWidth', 0.8);
axis equal;  % 保持坐标轴比例相等
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
%设置坐标轴名称的字体，可以覆盖上述设置
xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'origin','fitting'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthEast');
grid on;
